#include "WProgram.h"
//This is a Servo test
#include <Servo.h>

void setup();
void loop();
Servo servo1;
Servo servo2;
int servo1_port = 14;
int servo2_port = 15;
void setup()
{
  servo1.attach(servo1_port);
  servo1.setMaximumPulse(2200);
  servo2.attach(servo2_port);
  Serial.begin(9600);
  Serial.print("Ready");
}

void loop()
{
 int v = 0;

  if ( Serial.available()) {
    char ch = Serial.read();

    switch(ch) {
      case '0'...'9':
        v = v * 10 + ch - '0';
        break;
      case 's':
        servo1.write(v);
        v = 0;
        break;
      case 'w':
        servo2.write(v);
        v = 0;
        break;
      case 'd':
        servo2.detach();
        servo1.detach();
        break;
      case 'a':
        servo2.attach(15);
        servo1.attach(14);
        break;
    }
  }

  Servo::refresh();
}



int main(void)
{
	init();

	setup();
    
	for (;;)
		loop();
        
	return 0;
}

